#ifndef ARMOR_TRACKER__TRACKER_NODE_HPP_
#define ARMOR_TRACKER__TRACKER_NODE_HPP_

// ROS核心依赖
#include <message_filters/subscriber.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/create_timer_ros.h>
#include <tf2_ros/message_filter.h>
#include <tf2_ros/transform_listener.h>

#include <rclcpp/rclcpp.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

// 自定义接口与Tracker核心类
#include "armor_tracker/tracker.hpp"  // 包含ArmorTracker类定义
#include "aim_interfaces/msg/armors.hpp"
#include "aim_interfaces/msg/target.hpp"
#include "aim_interfaces/msg/tracker_info.hpp"

// STD依赖
#include <memory>
#include <string>
#include <vector>

namespace rm_aim
{
// TF2消息过滤器类型定义（过滤Armors消息）
using tf2_filter = tf2_ros::MessageFilter<aim_interfaces::msg::Armors>;

/**
 * @brief 装甲板追踪节点类
 * @note 负责ROS层数据交互与Tracker核心逻辑调度，不包含可视化功能
 */
class ArmorTrackerNode : public rclcpp::Node
{
public:
  /**
   * @brief 构造函数
   * @param options ROS节点选项（如参数重映射、命名空间等）
   * @note 初始化参数、Tracker、TF2、订阅者/发布者/服务
   */
  explicit ArmorTrackerNode(const rclcpp::NodeOptions & options);

private:
  /**
   * @brief 装甲板检测数据回调函数
   * @param armors_msg 装甲板检测消息指针
   * @note 处理流程：坐标转换→异常过滤→追踪更新→结果发布
   */
  void armorsCallback(const aim_interfaces::msg::Armors::SharedPtr armors_msg);

  // -------------------------- 核心参数与状态 --------------------------
  double max_armor_distance_;  // XOY平面最大装甲板距离（过滤用）
  rclcpp::Time last_time_;     // 上一帧数据的时间戳（计算时间间隔dt_）
  double dt_;                  // 两帧数据的时间间隔（用于EKF预测）
  double lost_time_thres_;     // 目标丢失时间阈值（秒）

  // Tracker状态阈值（稳定追踪所需连续匹配次数）
  int general_tracking_thres_;
  int outpost_tracking_thres_;
  // EKF过程噪声与观测通用噪声参数
  double general_sigma_q_xyz_;
  double general_sigma_q_yaw_;
  double general_sigma_q_r_;
  double general_r_xyz_factor_;
  double general_r_yaw_;
  // 前哨站模式参数
  double outpost_sigma_q_xyz_;
  double outpost_sigma_q_yaw_;
  double outpost_sigma_q_r_;
  double outpost_r_xyz_factor_;
  double outpost_r_yaw_;

  bool enable_debug_pub_;  // 调试发布开关

  // -------------------------- Tracker核心对象 --------------------------
  std::unique_ptr<ArmorTracker> tracker_;  // 装甲板追踪核心类实例

  // -------------------------- ROS服务 --------------------------
  // 重置追踪器服务（话题：/tracker/reset）
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reset_tracker_srv_;

  // -------------------------- TF2相关 --------------------------
  std::string target_frame_;                  // 目标坐标系（如odom）
  std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;       // TF2缓存对象
  std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;  // TF2监听器
  message_filters::Subscriber<aim_interfaces::msg::Armors> armors_sub_;  // 装甲板订阅者
  std::shared_ptr<tf2_filter> tf2_filter_;    // TF2消息过滤器（确保坐标可用）

  // -------------------------- ROS发布者 --------------------------
  rclcpp::Publisher<aim_interfaces::msg::TrackerInfo>::SharedPtr info_pub_;  // 调试信息发布者
  rclcpp::Publisher<aim_interfaces::msg::Target>::SharedPtr target_pub_;     // 核心目标发布者
};

}  // namespace rm_aim

#endif  // ARMOR_TRACKER__TRACKER_NODE_HPP_